package jp.seraph.robocup.soccer.research.sample.agent;

import java.io.IOException;
import java.text.ParseException;
import java.util.ArrayList;
import java.util.Calendar;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

import jp.seraph.jsade.net.RoboCupSocket;
import jp.seraph.jsade.sexpression.DefaultLeaf;
import jp.seraph.jsade.sexpression.DefaultNode;
import jp.seraph.jsade.sexpression.DefaultParser;
import jp.seraph.jsade.sexpression.ExpressionParser;
import jp.seraph.jsade.sexpression.Leaf;
import jp.seraph.jsade.sexpression.Node;
import jp.seraph.jsade.sexpression.SExpression;
import jp.seraph.jsade.sexpression.StringVisitor;

public class SampleNaoAgent {
    public SampleNaoAgent(RoboCupSocket aSocket) {
        mSocket = aSocket;
        mParser = DefaultParser.getInstance();
    }

    private static Map<String, JointID> JOINT_MAP;

    static {
        JOINT_MAP = new HashMap<String, JointID>();

        JOINT_MAP.put("hj1", JointID.HEAD_1);
        JOINT_MAP.put("hj2", JointID.HEAD_2);

        JOINT_MAP.put("llj1", JointID.LLEG_1);
        JOINT_MAP.put("rlj1", JointID.RLEG_1);
        JOINT_MAP.put("llj2", JointID.LLEG_2);
        JOINT_MAP.put("rlj2", JointID.RLEG_2);
        JOINT_MAP.put("llj3", JointID.LLEG_3);
        JOINT_MAP.put("rlj3", JointID.RLEG_3);
        JOINT_MAP.put("llj4", JointID.LLEG_4);
        JOINT_MAP.put("rlj4", JointID.RLEG_4);
        JOINT_MAP.put("llj5", JointID.LLEG_5);
        JOINT_MAP.put("rlj5", JointID.RLEG_5);
        JOINT_MAP.put("llj6", JointID.LLEG_6);
        JOINT_MAP.put("rlj6", JointID.RLEG_6);

        JOINT_MAP.put("laj1", JointID.LARM_1);
        JOINT_MAP.put("raj1", JointID.RARM_1);
        JOINT_MAP.put("laj2", JointID.LARM_2);
        JOINT_MAP.put("raj2", JointID.RARM_2);
        JOINT_MAP.put("laj3", JointID.LARM_3);
        JOINT_MAP.put("raj3", JointID.RARM_3);
        JOINT_MAP.put("laj4", JointID.LARM_4);
        JOINT_MAP.put("raj4", JointID.RARM_4);
    }

    private RoboCupSocket mSocket;
    private ExpressionParser mParser;
    private AgentState mState = AgentState.Wait;
    private int mCount = 0;
    private boolean mRunning;
    private boolean mInit = false;

    private List<SExpression> think(List<SExpression> aServerInput) {
        if (!mInit) {
            mInit = true;
            try {
                return mParser.parse("(init (unum 0)(teamname NaoRobot))");
            } catch (ParseException e) {
                e.printStackTrace();
                return null;
            }
        }

        for (SExpression tExpression : aServerInput) {
            if (tExpression.childCount() > 0 && tExpression.getChild(0).isLeaf()) {
                Leaf tFirstLeaf = tExpression.getChildAsLeaf(0);
                if (tFirstLeaf.getValue().equals("HJ")) {
                    parseHingeJointInfo((Node) tExpression);
                } else if (tFirstLeaf.getValue().equals("UJ")) {
                    parseUniversalJointInfo((Node) tExpression);
                } else if (tFirstLeaf.getValue().equals("hear")) {
                    parseHearInfo((Node) tExpression);
                } else {
                    System.out.println("処理できないS式 : " + tFirstLeaf.getValue());
                }
            }
        }

        List<SExpression> tResult = new ArrayList<SExpression>();

        switch (mState) {
        case Wait:
            if (mCount > 50) {
                mCount = 0;
                mState = AgentState.Sit;
            }
            break;
        case Sit:
            if (mCount > 40) {
                mCount = 0;
                mState = AgentState.SwingLeft;
                tResult.add(new DefaultNode(new DefaultLeaf("lae1"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("rae1"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("lae2"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("rae2"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("lae3"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("rae3"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("lae4"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("rae5"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("lle1"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("rle1"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("lle2"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("rle2"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("lle3"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("rle3"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("lle4"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("rle4"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("lle5"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("rle5"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("lle6"), new DefaultLeaf("0")));
                tResult.add(new DefaultNode(new DefaultLeaf("rle6"), new DefaultLeaf("0")));
                break;
            }
            tResult.add(new DefaultNode(new DefaultLeaf("lae1"), new DefaultLeaf("-1.5")));
            tResult.add(new DefaultNode(new DefaultLeaf("rae1"), new DefaultLeaf("-1.5")));
            tResult.add(new DefaultNode(new DefaultLeaf("lae2"), new DefaultLeaf("0.9")));
            tResult.add(new DefaultNode(new DefaultLeaf("rae2"), new DefaultLeaf("-0.9")));
            tResult.add(new DefaultNode(new DefaultLeaf("lae3"), new DefaultLeaf("0")));
            tResult.add(new DefaultNode(new DefaultLeaf("rae3"), new DefaultLeaf("0")));
            tResult.add(new DefaultNode(new DefaultLeaf("lae4"), new DefaultLeaf("-0.8")));
            tResult.add(new DefaultNode(new DefaultLeaf("rae5"), new DefaultLeaf("0.8")));
            tResult.add(new DefaultNode(new DefaultLeaf("lle3"), new DefaultLeaf("1")));
            tResult.add(new DefaultNode(new DefaultLeaf("rle3"), new DefaultLeaf("1")));
            tResult.add(new DefaultNode(new DefaultLeaf("lle4"), new DefaultLeaf("-2")));
            tResult.add(new DefaultNode(new DefaultLeaf("rle4"), new DefaultLeaf("-2")));
            tResult.add(new DefaultNode(new DefaultLeaf("lle5"), new DefaultLeaf("1")));
            tResult.add(new DefaultNode(new DefaultLeaf("rle5"), new DefaultLeaf("1")));
            tResult.add(new DefaultNode(new DefaultLeaf("lle6"), new DefaultLeaf("0")));
            tResult.add(new DefaultNode(new DefaultLeaf("rle6"), new DefaultLeaf("0")));
            break;
        case SwingLeft:
            if (mCount > 50) {
                mCount = 0;
                mState = AgentState.SwingRight;
                break;
            }
            tResult.add(new DefaultNode(new DefaultLeaf("he1"), new DefaultLeaf("-1")));
            break;
        case SwingRight:
            if (mCount > 50) {
                mCount = 0;
                mState = AgentState.SwingLeft;
                break;
            }
            tResult.add(new DefaultNode(new DefaultLeaf("he1"), new DefaultLeaf("1")));
            break;

        default:
            break;
        }

        mCount++;
        if (mCount % 10 == 0)
            tResult.add(new DefaultNode(new DefaultLeaf("say"), new DefaultLeaf("ComeOn!")));
        return tResult;
    }

    private void parseHingeJointInfo(Node aExpression) {
        String tName = aExpression.getValue("n");
        if (tName == null)
            return;
        if (!JOINT_MAP.containsKey(tName)) {
            System.out.println("unknown joint id:" + tName);
        }
        // JointID tID = JOINT_MAP.get(tName);

        // 実装しないことにした
    }

    private void parseUniversalJointInfo(Node aExpression) {
        // 実装しないことにした
    }

    private void parseHearInfo(Node aExpression) {
        double tHearTime;
        String tSender;
        String tMessage;

        if (aExpression.childCount() != 4) {
            System.out.println("hearパーセプタの要素数は4でなければいけません。");
            return;
        }

        tHearTime = Double.parseDouble(aExpression.getChildAsLeaf(1).getValue());
        tSender = aExpression.getChildAsLeaf(2).getValue();
        tMessage = aExpression.getChildAsLeaf(3).getValue();

        if ("self".equals(tSender)) {
            System.out.println("I said " + tMessage + " at " + tHearTime);
        } else {
            String tPosition = Double.parseDouble(tSender) < 90 ? "in fromt of" : "behind";
            System.out.println("Someone " + tPosition + " me said " + tMessage + " at " + tHearTime);
        }
    }

    public void run() {
        mRunning = true;
        this.init();
        System.out.println("Running.");

        try {
            List<SExpression> tInit = mParser.parse("(init (unum 0)(teamname NaoRobot))");
            StringVisitor tInitVisitor = new StringVisitor();
            for (SExpression tExpression : tInit)
                tExpression.accept(tInitVisitor);
            mSocket.write(tInitVisitor.getString());
            mInit = true;
        } catch (Exception e) {
            e.printStackTrace();
            return;
        }

        while (mRunning) {
            try {
                String tReceive = mSocket.read();
                System.out.print(Calendar.getInstance().get(Calendar.SECOND));
                System.out.print(".");
                System.out.printf("%03d%n", Calendar.getInstance().get(Calendar.MILLISECOND));
                System.out.println("Receive : " + tReceive);

                List<SExpression> tActions = this.think(mParser.parse(tReceive));
                if (tActions == null) {
                    System.out.print("アクションにnull返ってきてるぜ…？");
                    mRunning = false;
                    break;
                }

                StringVisitor tVisitor = new StringVisitor();
                for (SExpression tExpression : tActions)
                    tExpression.accept(tVisitor);
                mSocket.write(tVisitor.getString());
            } catch (IOException e) {
                e.printStackTrace();
                mRunning = false;
            } catch (ParseException e) {
                e.printStackTrace();
                mRunning = false;
            }
        }

        mSocket.close();
        System.out.println("close connection.");
    }

    public boolean init() {

        try {
            mSocket.write("(scene rsg/agent/nao/nao.rsg)");
        } catch (IOException e) {
            e.printStackTrace();
            return false;
        }

        mRunning = true;
        return true;
    }

    private enum AgentState {
        Wait,
        Sit,
        SwingLeft,
        SwingRight,
    }
}
